﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace StaticView.Algorithm
{
    struct Matrix
    {
        float[,] M;
        public int Row { get; private set; }
        public int Col { get; private set; }

        public Matrix(int row, int col)
            : this()
        {
            M = new float[row, col];
            Row = row;
            Col = col;
        }
        public Matrix(int size)
            : this()
        {
            M = new float[size, size];
            Row = Col = size;
        }
        public Matrix(Microsoft.Xna.Framework.Vector3 v)
            : this()
        {
            M = new float[3, 1];
            Row = 3; Col = 1;
            M[0, 0] = v.X; M[1, 0] = v.Y; M[2, 0] = v.Z;
        }
        public Matrix(float[] data)
            : this()
        {
            M = new float[data.Length, 1];
            Row = data.Length; Col = 1;
            for (int i = 0; i < Row; i++)
                M[i, 0] = data[i];
        }

        public float this[int n, int m]
        {
            get { return M[n, m]; }
            set { M[n, m] = value; }
        }

        public void setData(float[] data)
        {
            for (int n = 0; n < Row; n++)
                for (int m = 0; m < Col; m++)
                    M[n, m] = data[n * Col + m];
        }

        public override string ToString()
        {
            string str = "";
            for (int n = 0; n < Row; n++)
            {
                str += "| ";
                for (int m = 0; m < Col; m++)
                    str += String.Format("{0, 5} | ", M[n, m]);
                str += "\n";
            }
            return str;
        }

        public float[] ToFloatArray()
        {
            if (Col != 1)
                throw new MatrixException("Float conversion: Invalid size");
            float[] data = new float[Row];
            for (int i = 0; i < Row; i++)
                data[i] = M[i, 0];
            return data;
        }

        public Microsoft.Xna.Framework.Vector3 ToVector3()
        {
            if (Col != 1 || Row != 3)
                throw new MatrixException("Vector3 conversion: Invalid size");
            Microsoft.Xna.Framework.Vector3 data = new Microsoft.Xna.Framework.Vector3(M[0, 0], M[1, 0], M[2, 0]);
            return data;
        }

        public static Matrix Identity(int size)
        {
            Matrix I = new Matrix(size);
            for (int i = 0; i < size; i++)
                I[i, i] = 1;
            return I;
        }

        public static Matrix operator +(Matrix a, Matrix b)
        {
            if (a.Row != b.Row || a.Col != b.Col)
                throw new MatrixException("Add operator: Invalid size");
            Matrix c = new Matrix(a.Row, a.Col);
            for (int n = 0; n < a.Row; n++)
                for (int m = 0; m < a.Col; m++)
                    c[n, m] = a[n, m] + b[n, m];
            return c;
        }
        public static Matrix operator -(Matrix a, Matrix b)
        {
            if (a.Row != b.Row || a.Col != b.Col)
                throw new MatrixException("Subtract operator: Invalid size");
            Matrix c = new Matrix(a.Row, a.Col);
            for (int n = 0; n < a.Row; n++)
                for (int m = 0; m < a.Col; m++)
                    c[n, m] = a[n, m] - b[n, m];
            return c;
        }
        public static Matrix operator *(Matrix a, Matrix b)
        {
            if (a.Col != b.Row)
                throw new MatrixException("Multiply operator: Invalid size");
            Matrix c = new Matrix(a.Row, b.Col);
            for (int n = 0; n < a.Row; n++)
                for (int m = 0; m < b.Col; m++)
                {
                    c[n, m] = 0;
                    for (int k = 0; k < a.Col; k++)
                        c[n, m] += a[n, k] * b[k, m];
                }
            return c;
        }
        public static Matrix operator *(Matrix a, float b)
        {
            Matrix c = new Matrix(a.Row, a.Col);
            for (int n = 0; n < a.Row; n++)
                for (int m = 0; m < a.Col; m++)
                    c[n, m] = a[n, m] * b;
            return c;
        }
        public static Matrix operator /(Matrix a, float b)
        {
            if (b == 0)
                throw new MatrixException("Divide operator: Divide by zero");
            Matrix c = new Matrix(a.Row, a.Col);
            for (int n = 0; n < a.Row; n++)
                for (int m = 0; m < a.Col; m++)
                    c[n, m] = a[n, m] / b;
            return c;
        }

        public Matrix Transpose()
        {
            Matrix T = new Matrix(Col, Row);
            for (int n = 0; n < Row; n++)
                for (int m = 0; m < Col; m++)
                    T[m, n] = M[n, m];
            return T;
        }

        public Matrix Invert()
        {
            if (Row != Col)
                throw new MatrixException("Invert operator: Invalid size");
            // Gauss Elimination
            Matrix G = new Matrix(Row, 2 * Row);
            for (int n = 0; n < Row; n++)
            {
                for (int m = 0; m < Row; m++)
                    G[n, m] = M[n, m];
                G[n, n + Row] = 1;
            }
            for (int i = 0; i < Row - 1; i++)
            {
                // Find another line to swap if the current line is invalid
                if (G[i, i] == 0)
                {
                    for (int j = i + 1; j < Row; j++)
                    {
                        if (G[j, i] != 0)
                        {
                            for (int k = i; k < 2 * Row; k++)
                            {
                                float t = G[i, k];
                                G[i, k] = G[j, k];
                                G[j, k] = t;
                            }
                            break;
                        }
                        // There are no line to swap
                        if (j == Row - 1)
                            throw new MatrixException("Invert operator: Matrix can't be reduced, Insufficient parameters");
                    }
                }
                // Eliminating
                for (int j = i + 1; j < Row; j++)
                {
                    if (G[j, i] == 0) continue;
                    for (int k = i + 1; k < 2 * Row; k++)
                        G[j, k] -= G[i, k] * G[j, i] / G[i, i];
                    G[j, i] = 0;
                }
            }
            // Test for the last line
            if (G[Row - 1, Row - 1] == 0)
                throw new MatrixException("Invert operator: Matrix can't be reduced, Invalid last line");
            // Echelon form, continue eliminating
            for (int i = Row - 1; i >= 0; i--)
            {
                for (int j = 0; j < i; j++)
                {
                    for (int k = Row; k < 2 * Row; k++)
                        G[j, k] -= G[i, k] * G[j, i] / G[i, i];
                    G[j, i] = 0;
                }
                // Reduce
                for (int k = Row; k < 2 * Row; k++)
                    G[i, k] /= G[i, i];
                G[i, i] = 1;
            }
            Matrix B = new Matrix(Row, Row);
            for (int n = 0; n < Row; n++)
                for (int m = 0; m < Row; m++)
                    B[n, m] = G[n, m + Row];
            return B;
        }
    }

    class MatrixException : Exception
    {
        public MatrixException() : base() { }
        public MatrixException(string message) : base(message) { }
    }
}
